/*
Copyright (C) 2016  Marien Raat

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Affero General Public License as
published by the Free Software Foundation, either version 3 of the
License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Affero General Public License for more details.

You should have received a copy of the GNU Affero General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "wheel.h"
#include "config.h"

#include <math.h>
#include <iostream>

extern float timeStep;

void Wheel::initialize(b2Body *carBody, b2Vec2 relativePosition)
{
    this->carBody = carBody;
    this->relativePosition = relativePosition;
    currentRotation = 0.0f;
}

void Wheel::update(bool drifting, bool dashing)
{
    this->drifting = drifting;
    this->dashing = dashing;
    // Variables determined by the current state (drifting)
    float perpendicularFriction, maxPerpendicularFrictionImpulse,
        parallelFriction, angularDamping, turnRestore;

    if (!drifting && !dashing) {
        perpendicularFriction = 0.25f * 0.8f * 60.0f;
        maxPerpendicularFrictionImpulse = 5.25f * 60.0f;
        parallelFriction = 0.25f * 0.02f * 60.0f;
        angularDamping = 0.25f * 0.4f * 60.0f;
        turnRestore = 0.0f;
    } else if (dashing) {
        perpendicularFriction = 0.25f * 0.02f * 60.0f;
        maxPerpendicularFrictionImpulse = 100.0f * 60.0f;
        parallelFriction = 0.25f * 0.015f * 60.0f;
        angularDamping = 0.25f * 0.1f * 60.0f;
        turnRestore = 0.0f;
    } else { // drifting
        perpendicularFriction = 0.25f * 0.8f * 60.0f;
        maxPerpendicularFrictionImpulse = 5.25f * 60.0f;
        parallelFriction = 0.25f * 0.025f * 60.0f;
        angularDamping = 0.25f * 0.18f * 60.0f;
        turnRestore = 0.0f;
    }
    
    b2Vec2 velocity = carBody->GetLinearVelocity();
    float carAngle = carBody->GetAngle();
    b2Vec2 wheelPosition = ROTATE_B2VEC2(relativePosition, carAngle) +
        carBody->GetWorldCenter();

    float angle = carAngle + currentRotation;
    b2Vec2 perpendicularVelocity =
        b2Dot(ROTATE_B2VEC2(b2Vec2(0, 1), angle),
              velocity) * ROTATE_B2VEC2(b2Vec2(0, 1), angle);
   if (perpendicularVelocity.Length() >
       maxPerpendicularFrictionImpulse) {
       perpendicularVelocity.Normalize();
       perpendicularVelocity *= maxPerpendicularFrictionImpulse;
   }

    b2Vec2 parallelVelocity =
        b2Dot(ROTATE_B2VEC2(b2Vec2(1, 0), angle),
              velocity) * ROTATE_B2VEC2(b2Vec2(1, 0), angle);

    carBody->ApplyLinearImpulse(-1.0f * timeStep *
                                perpendicularFriction *
                                perpendicularVelocity,
                                wheelPosition, true);

    carBody->ApplyLinearImpulse(-1.0f * timeStep * parallelFriction *
                                parallelVelocity,
                                wheelPosition, true);

    carBody->ApplyAngularImpulse(-angularDamping *
                                 carBody->GetInertia() *
                                 carBody->GetAngularVelocity() *
                                 timeStep, true);

    currentRotation *= turnRestore;
}

void Wheel::move(float forward)
{
    float force = !drifting ? 20 : 5;
    if (dashing) force = 25;
    
    b2Vec2 wheelPosition = ROTATE_B2VEC2(relativePosition,
                                        carBody->GetAngle()) +
        carBody->GetWorldCenter();
    
    float angle = carBody->GetAngle() + currentRotation;
    carBody->ApplyForce(ROTATE_B2VEC2(
                            b2Vec2(1.0f * forward * force, 0),
                            angle),
                        wheelPosition, true);
}

void Wheel::turn(float turning)
{
    float turnAdd = (!drifting ? 12.0f: 18.0f) * turning * DEG_TO_RAD;
    if (dashing) turnAdd = 30.0f * turning * DEG_TO_RAD;
    currentRotation += turnAdd;
}

sf::Packet &operator <<(sf::Packet &packet, Wheel &wheel)
{
    packet << wheel.currentRotation;
}

sf::Packet &operator >>(sf::Packet &packet, Wheel &wheel)
{
    packet >> wheel.currentRotation;
}
